// Pines A
const int AIN1 = 5;
const int AIN2 = 4;
const int PWMA = 3;

// Pines B
const int BIN1 = 6;
const int BIN2 = 7;
const int PWMB = 9;

const int STBY = 8;

// Velocidad PWM (0-255)
//const int velocidad = 255;
const int pasosTotales = 2500 ; // 8 mm de recorrido
const int delayPaso = 1; // Tiempo entre pasos en ms

// Secuencia de pasos completos
const int secuencia[4][4] = {
  {1, 0, 1, 0}, // Paso 1
  {0, 1, 1, 0}, // Paso 2
  {0, 1, 0, 1}, // Paso 3
  {1, 0, 0, 1}  // Paso 4
};

void setup() {
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);

  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  pinMode(STBY, OUTPUT);
  digitalWrite(STBY, HIGH); // Activa el driver

  Serial.begin(9600);
  Serial.println("Listo para recibir comandos: f (forward), b (backward), s (stop)");
}

void loop() {
  if (Serial.available()) {
    char comando = Serial.read();
    if (comando == 'f') {
      Serial.println("Avanzando...");
      moverPasos(pasosTotales, delayPaso);
    } else if (comando == 'b') {
      Serial.println("Retrocediendo...");
      moverPasos(-pasosTotales, delayPaso);
    } else if (comando == 's') {
      Serial.println("Detenido.");
      apagarMotor();
    }
  }
}

void moverPasos(int pasos, int delayPaso) {
  int dir = pasos > 0 ? 1 : -1;
  pasos = abs(pasos);

  for (int i = 0; i < pasos; i++) {
    int idx = (i * dir) % 4;
    if (idx < 0) idx += 4;
    setStep(secuencia[idx]);
    delay(delayPaso);
  }

  apagarMotor();
}

void setStep(const int paso[4]) {
  digitalWrite(AIN1, paso[0]);
  digitalWrite(AIN2, paso[1]);
  digitalWrite(BIN1, paso[2]);
  digitalWrite(BIN2, paso[3]);
  //analogWrite(PWMA, velocidad);
  //analogWrite(PWMB, velocidad);
  digitalWrite(PWMA,HIGH);
  digitalWrite(PWMB,HIGH);
}

void apagarMotor() {
  digitalWrite(PWMA,LOW);
  digitalWrite(PWMB,LOW);
}